{
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "language": "python",
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "nbconvert_exporter": "python",
      "file_extension": ".py",
      "name": "python",
      "version": "3.5.2",
      "mimetype": "text/x-python",
      "pygments_lexer": "ipython3",
      "codemirror_mode": {
        "version": 3,
        "name": "ipython"
      }
    }
  },
  "nbformat": 4,
  "cells": [
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "outputs": [],
      "execution_count": null,
      "source": [
        "%matplotlib inline"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "\n********************************************************************************\nPendulum Example\n********************************************************************************\nThe set of all points in the Euclidean space $\\mathbb{R}^{3}$, that lie on\nthe surface of the unit ball about the origin belong to the two-sphere manifold,\n\n\\begin{align}\\mathbb{S}^2 = \\left\\{ \\mathbf{x} \\in \n    \\mathbb{R}^3 \\mid \\|\\mathbf{x}\\|_2 = 1\n    \\right\\},\\end{align}\n\nwhich is a two-dimensional manifold. Many mechanical systems such as a spherical\npendulum, double pendulum, quadrotor with a cable-suspended load, evolve on\neither $\\mathbb{S}^2$ or products comprising of $\\mathbb{S}^2$.\n\nIn this script, we estimate the state of a system living on the sphere but where\nobservations are standard vectors. See the description of the spherical pendulum\ndynamics in :cite:`sjobergAn2019`, Section 7, and :cite:`kotaruVariation2019`.\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Import\n==============================================================================\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "outputs": [],
      "execution_count": null,
      "source": [
        "from scipy.linalg import block_diag\nimport ukfm\nimport numpy as np\nimport matplotlib\nukfm.utils.set_matplotlib_config()"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Model and Simulation\n==============================================================================\nThis script uses the :meth:`~ukfm.PENDULUM` model that requires  the sequence\ntime and the model frequency.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "outputs": [],
      "execution_count": null,
      "source": [
        "# sequence time (s)\nT = 10\n# model frequency (Hz)\nmodel_freq = 100\n# create the model\nmodel = ukfm.PENDULUM(T, model_freq)"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "The true trajectory is computed along with empty inputs (the model does not\nrequire any input) after we define the noise standard deviation affecting the\ndynamic.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "outputs": [],
      "execution_count": null,
      "source": [
        "# model noise standard deviation (noise is isotropic)\nmodel_std = np.array([1/180*np.pi,    # orientation (rad)\n                      1/180*np.pi])   # orientation velocity (rad/s)\n# simulate true states and noisy inputs\nstates, omegas = model.simu_f(model_std)"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "The state and the input contain the following variables:\n\n.. highlight:: python\n.. code-block:: python\n\n  states[n].Rot  # 3d orientation (matrix)\n  states[n].u    # 3d angular velocity\n  omegas[n]      # empty input\n\nThe model dynamics is based on the Euler equations of pendulum motion.\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "We compute noisy measurements at low frequency based on the true states.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "outputs": [],
      "execution_count": null,
      "source": [
        "# observation frequency (Hz)\nobs_freq = 20\n# observation noise standard deviation (m)\nobs_std = 0.02\n# simulate landmark measurements\nys, one_hot_ys = model.simu_h(states, obs_freq, obs_std)"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "We assume observing the position of the state only in the $yz$-plan.\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Filter Design and Initialization\n------------------------------------------------------------------------------\nWe embed the state in $SO(3) \\times \\mathbb{R} ^3$ with left\nmultiplication, such that:\n\n- the retraction $\\varphi(.,.)$ is the $SO(3)$ exponential for\n  orientation where the state multiplies the uncertainty on the left, and the\n  vector addition for the velocity.\n\n- the inverse retraction $\\varphi^{-1}_.(.)$ is the $SO(3)$\n  logarithm for orientation and the vector subtraction for the velocity.\n\nRemaining parameter setting is standard.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "outputs": [],
      "execution_count": null,
      "source": [
        "# propagation noise covariance matrix\nQ = block_diag(model_std[0]**2*np.eye(3), model_std[1]**2*np.eye(3))\n# measurement noise covariance matrix\nR = obs_std**2*np.eye(2)\n# initial uncertainty matrix\nP0 = block_diag((45/180*np.pi)**2*np.eye(3), (10/180*np.pi)**2*np.eye(3))\n# sigma point parameters\nalpha = np.array([1e-3, 1e-3, 1e-3])\nstate0 = model.STATE(Rot=np.eye(3), u=np.zeros(3))\nukf = ukfm.UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n               phi=model.phi, phi_inv=model.phi_inv, alpha=alpha)\n# set variables for recording estimates along the full trajectory\nukf_states = [state0]\nukf_Ps = np.zeros((model.N, 6, 6))\nukf_Ps[0] = P0"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Filtering\n==============================================================================\nThe UKF proceeds as a standard Kalman filter with a for loop.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "outputs": [],
      "execution_count": null,
      "source": [
        "# measurement iteration number\nk = 1\nfor n in range(1, model.N):\n    # propagation\n    ukf.propagation(omegas[n-1], model.dt)\n    # update only if a measurement is received\n    if one_hot_ys[n] == 1:\n        ukf.update(ys[k])\n        k = k + 1\n    # save estimates\n    ukf_states.append(ukf.state)\n    ukf_Ps[n] = ukf.P"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Results\n------------------------------------------------------------------------------\nWe plot the position of the pendulum as function of time, the position in the\n$xy$ plan and the position in the $yz$ plan (we are more\ninterested in the position of the pendulum than its orientation). We compute\nthe $3\\sigma$ interval confidence by leveraging the *covariance\nretrieval* proposed in :cite:`brossardCode2019`, Section V-B.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "outputs": [],
      "execution_count": null,
      "source": [
        "model.plot_results(ukf_states, ukf_Ps, states)"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "On the first plot, we observe that even if the state is unaccurately\ninitialized, the filter estimates the depth position ($x$ axis) of the\npendulum whereas only the $yz$ position of the pendulum is observed.\n\nThe second and third plots show how the filter converges to the true state.\nFinally, the last plot reveals the consistency of the filter, where the\ninterval confidence encompasses the error.\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Conclusion\n==============================================================================\nThis script shows how well works the UKF on parallelizable manifolds for\nestimating the position of a spherical pendulum where only two components of\nthe pendulum are measured. The filter is accurate, robust to strong initial\nerrors, and obtains consistent covariance estimates with the method proposed\nin :cite:`brossardCode2019`.\n\nYou can now:\n\n- address the same problem with another retraction, e.g. with right\n  multiplication.\n\n- modify the measurement with 3D position.\n\n- consider the mass of the system as unknown and estimate it.\n\n"
      ]
    }
  ]
}